23. Predicted Mean and Covariance Assignment 1
Assignement Predicted Mean And Covariance
Predicted Mean and Covariance Assingment
Task Description:
You will be completing the missing code in ukf.cpp, PredictMeanAndCovariance() function.
Task Feedback:
Nice! Have you tested your code below?
Helpful Equations
Weights
\Large w_i =\frac{\lambda}{\lambda+n_{a}}, i =1
\Large w_i =\frac{1}{2(\lambda+n_{a})}, i =2…n_{a}
Predicted Mean
\Large x_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i X_{k+1|k,i}
Predicted Covariance
\Large P_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i( X_{k+1|k,i} - x_{k+1|k})(X_{k+1|k,i} - x_{k+1|k})^T
Start Quiz:
#include "Dense"
#include "ukf.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
int main() {
// Create a UKF instance
UKF ukf;
/**
* Programming assignment calls
*/
VectorXd x_pred = VectorXd(5);
MatrixXd P_pred = MatrixXd(5, 5);
ukf.PredictMeanAndCovariance(&x_pred, &P_pred);
return 0;
}
#include <iostream>
#include "ukf.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
UKF::UKF() {
Init();
}
UKF::~UKF() {
}
void UKF::Init() {
}
/**
* Programming assignment functions:
*/
void UKF::PredictMeanAndCovariance(VectorXd* x_out, MatrixXd* P_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// define spreading parameter
double lambda = 3 - n_aug;
// create example matrix with predicted sigma points
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
// create vector for predicted state
VectorXd x = VectorXd(n_x);
// create covariance matrix for prediction
MatrixXd P = MatrixXd(n_x, n_x);
/**
* Student part begin
*/
// set weights
// predict state mean
// predict state covariance matrix
/**
* Student part end
*/
// print result
std::cout << "Predicted state" << std::endl;
std::cout << x << std::endl;
std::cout << "Predicted covariance matrix" << std::endl;
std::cout << P << std::endl;
// write result
*x_out = x;
*P_out = P;
}
#ifndef UKF_H
#define UKF_H
#include "Dense"
class UKF {
public:
/**
* Constructor
*/
UKF();
/**
* Destructor
*/
virtual ~UKF();
/**
* Init Initializes Unscented Kalman filter
*/
void Init();
/**
* Student assignment functions
*/
void GenerateSigmaPoints(Eigen::MatrixXd* Xsig_out);
void AugmentedSigmaPoints(Eigen::MatrixXd* Xsig_out);
void SigmaPointPrediction(Eigen::MatrixXd* Xsig_out);
void PredictMeanAndCovariance(Eigen::VectorXd* x_pred,
Eigen::MatrixXd* P_pred);
void PredictRadarMeasurement(Eigen::VectorXd* z_out,
Eigen::MatrixXd* S_out);
void UpdateState(Eigen::VectorXd* x_out,
Eigen::MatrixXd* P_out);
};
#endif // UKF_H
expected result x:
x =
5.93637
1.49035
2.20528
0.536853
0.353577
expected result p:
P =
0.00543425 -0.0024053 0.00341576 -0.00348196 -0.00299378
-0.0024053 0.010845 0.0014923 0.00980182 0.00791091
0.00341576 0.0014923 0.00580129 0.000778632 0.000792973
-0.00348196 0.00980182 0.000778632 0.0119238 0.0112491
-0.00299378 0.00791091 0.000792973 0.0112491 0.0126972